package jflowmap.util.box2d;

import java.awt.geom.Point2D;
import java.util.Collections;
import java.util.Iterator;
import java.util.List;
import java.util.Vector;

import org.jbox2d.collision.shapes.CircleShape;
import org.jbox2d.collision.shapes.PolygonShape;
import org.jbox2d.collision.shapes.Shape;
import org.jbox2d.collision.shapes.ShapeType;
import org.jbox2d.common.Vec2;
import org.jbox2d.dynamics.Body;
import org.jbox2d.dynamics.BodyDef;
import org.jbox2d.dynamics.BodyType;
import org.jbox2d.dynamics.Fixture;
import org.jbox2d.dynamics.FixtureDef;
import org.jbox2d.dynamics.World;
import org.jbox2d.dynamics.joints.DistanceJoint;
import org.jbox2d.dynamics.joints.DistanceJointDef;

public class DynamicPlacer implements Iterable<DynamicallyPlaceable> {
	
	private final List<DynamicallyPlaceable> placeableObjects = new Vector<DynamicallyPlaceable>();
	private final static float BOX2D_RATIO = (float)(1.0/20);
	
	private final List<Body> bodies = new Vector<Body>();
    private final List<Body> anchorBodies = new Vector<Body>();
    
    private final List<Fixture> fixtures = new Vector<Fixture>();
    private final List<Fixture> anchorFixtures = new Vector<Fixture>();
    private final List<DistanceJoint> joints = new Vector<DistanceJoint>();
    
    private World world;
    
    public DynamicPlacer() {
    	world = new World(new Vec2(0, 0), true);
    }
    
	public void add (DynamicallyPlaceable placeable) {
		placeableObjects.add(placeable);
	}
	
	public void remove (DynamicallyPlaceable placeable) {
		placeableObjects.remove(placeable);
	}
	
	public void clear () {
		placeableObjects.clear();
	}

	private void resetWorld () {
		bodies.clear();
		anchorBodies.clear();
		fixtures.clear();
		anchorFixtures.clear();
		joints.clear();
		world = new World(new Vec2(0, 0), true);
	}
	
	@Override
	public Iterator<DynamicallyPlaceable> iterator() {
		return Collections.unmodifiableList(placeableObjects).iterator();
	}
	
	public void calculatePositions () {
		resetWorld();
		createBodies();
        for (int i = 0; i < 500; ++i) {
            world.step(1/50.0f, 12, 6);
        }
	}
	
	public void updatePositions () {
		for (int i = 0; i < placeableObjects.size(); ++i) {
			Body body = bodies.get(i);
			final Vec2 p = body.getWorldCenter();
			DynamicallyPlaceable placeable = placeableObjects.get(i);
			placeable.placeAt(new Point2D.Double(p.x/BOX2D_RATIO, p.y/BOX2D_RATIO));
		}
	}
	
	private Point2D getGlobalCenter () {
		Point2D center = new Point2D.Double(0, 0);
		for (DynamicallyPlaceable placeable: placeableObjects) {
			Point2D pos = placeable.getGlobalOrigin();
			center.setLocation(center.getX() + pos.getX()*BOX2D_RATIO, center.getY() + pos.getY()*BOX2D_RATIO);
		}
		final int size = placeableObjects.size();
		center.setLocation(center.getX()/size, center.getY()/size);
		return center;
	}
	
	private void createBodies () {
		final Point2D center = getGlobalCenter();
		
		for (DynamicallyPlaceable placeable: placeableObjects) {
			BodyDef bd = new BodyDef();
			bd.type = BodyType.DYNAMIC;
			bd.fixedRotation = true;
			bd.linearDamping = 1.0f;
			
			Point2D position = placeable.getGlobalOrigin();
			
			Point2D bodyPosition = new Point2D.Double(position.getX()*BOX2D_RATIO, position.getY()*BOX2D_RATIO);
			final double dx = bodyPosition.getX() - center.getX();
			final double dy = bodyPosition.getY() - center.getY();
			bodyPosition.setLocation(center.getX() + dx*3.0f, center.getY() + dy*3.0f);
			bd.position.set((float)bodyPosition.getX(), (float)bodyPosition.getY());
			final Body body = world.createBody(bd);
			bodies.add(body);
			
			BodyDef anchorDef = new BodyDef();
			anchorDef.type = BodyType.STATIC;
			anchorDef.position.set((float) position.getX()*BOX2D_RATIO, (float) position.getY()*BOX2D_RATIO);
			final Body anchorBody = world.createBody(anchorDef);
			anchorBodies.add(anchorBody);
			
			Shape shape = null;
			
			if (placeable.getShapeType() == DynamicallyPlaceable.ShapeType.CIRCLE) {
				CircleShape circle = new CircleShape();
				double radius = placeable.getShapeRadius();
				//if (radius < 50) radius = 50;
				circle.m_radius = (float)(radius*BOX2D_RATIO);
				circle.m_type = ShapeType.CIRCLE;
				circle.m_p.set(bd.position);
				shape = circle;
				
			} else {
				final PolygonShape rect = new PolygonShape();

				rect.setAsBox((float) placeable.getShapeWidth()*BOX2D_RATIO,
							  (float) placeable.getShapeHeight()*BOX2D_RATIO);
				rect.m_type = ShapeType.POLYGON;
				rect.m_centroid.set(bd.position);
				shape = rect;
			}
			
			final FixtureDef fd = new FixtureDef();
			fd.friction = 0.8f;
			fd.density = (float)placeable.getMass();//0.1f;
			fd.restitution = 0.1f;
			fd.shape = shape;
			fd.filter.categoryBits = 2;
			fd.filter.maskBits = 2;
			final Fixture fixture = body.createFixture(fd);
			fixtures.add(fixture);
			
			final CircleShape anchorCircle = new CircleShape();
			anchorCircle.m_radius = 2.5f;
			final FixtureDef anchorFixtureDef = new FixtureDef();
			anchorFixtureDef.shape = anchorCircle;
			anchorFixtureDef.friction = 0.8f;
			anchorFixtureDef.density = 1.0f;
			anchorFixtureDef.restitution = 0.1f;
			anchorFixtureDef.filter.categoryBits = 4;
			anchorFixtureDef.filter.maskBits = 4;
			final Fixture anchorFixture = anchorBody.createFixture(anchorFixtureDef);
			anchorFixtures.add(anchorFixture);
			
			final DistanceJointDef jointDef = new DistanceJointDef();
			jointDef.initialize(body, anchorBody, body.getWorldCenter(), anchorBody.getWorldCenter());
			jointDef.collideConnected = true;
			jointDef.length = 0;
			jointDef.frequencyHz = 2.0f;
			jointDef.dampingRatio = 0.5f;
			final DistanceJoint joint = (DistanceJoint) world.createJoint(jointDef);
			joints.add(joint);
		}
	}
}
